#ifndef FuzzyPIDController_h
#define FuzzyPIDController_h
#include <vector>
#include <string>

class FuzzySet {
public:
    FuzzySet(std::string name, double min, double max, double a, double b, double c, double d);

    double membership(double x) const;
    std::string get_name() const;
    double get_a() const;
    double get_b() const;
    double get_c() const;
    double get_d() const;
    double get_min() const;
    double get_max() const;

private:
    std::string name_;
    double a_;
    double b_;
    double c_;
    double d_;
    double min_;
    double max_;

};

class FuzzyPIDController {
public:
    FuzzyPIDController(double err_min, double err_max, double err_change_rate_min, double err_change_rate_max, double foutput_min, double foutput_max,
        double Kp, double Ki, double Kd);

    void add_err_set(std::string name, double a, double b, double c, double d);
    void add_err_change_rate_set(std::string name, double a, double b, double c, double d);
    void add_output_set(std::string name, double a, double b, double c, double d);
    void add_rule(std::string err_set_name, std::string err_change_rate_set_name, std::string output_set_name);

    double infer(double err, double err_change_rate);
    void set_setpoint(double setpoint);
    void set_output_limits(double foutput_min, double foutput_max);
    double setpoint_;
    double p_ = 0;
    double i_ = 0;
    double d_ = 0;

private:
    double err_min_;
    double err_max_;
    double err_change_rate_min_;
    double err_change_rate_max_;
    double output_min_;
    double output_max_;
    double Kp_;
    double Ki_;
    double Kd_;
    std::vector<FuzzySet> err_sets_;
    std::vector<FuzzySet> err_change_rate_sets_;
    std::vector<FuzzySet> output_sets_;
    std::vector<std::vector<int>> rules_;
    double foutput_min_;
    double foutput_max_;

};

#endif